-
Notifications
You must be signed in to change notification settings - Fork 639
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
feat: add 'obstacle_velocity_limiter' package #1579
feat: add 'obstacle_velocity_limiter' package #1579
Conversation
Codecov ReportBase: 10.77% // Head: 12.56% // Increases project coverage by
Additional details and impacted files@@ Coverage Diff @@
## main #1579 +/- ##
==========================================
+ Coverage 10.77% 12.56% +1.79%
==========================================
Files 1186 731 -455
Lines 84834 59940 -24894
Branches 19889 18609 -1280
==========================================
- Hits 9138 7531 -1607
+ Misses 66012 43636 -22376
+ Partials 9684 8773 -911
Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here. ☔ View full report at Codecov. |
772ec16
to
09e9ec4
Compare
22b4378
to
1795ede
Compare
c523855
to
cff4a52
Compare
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
Signed-off-by: Maxime CLEMENT <[email protected]>
launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml
Outdated
Show resolved
Hide resolved
planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp
Outdated
Show resolved
Hide resolved
planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml
Outdated
Show resolved
Hide resolved
Signed-off-by: Maxime CLEMENT <[email protected]>
69238c9
to
2fad4dc
Compare
Signed-off-by: Maxime CLEMENT <[email protected]>
LGTM 👍 |
Signed-off-by: Maxime CLEMENT <[email protected]>
3413912
to
f547a8f
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It worked in my environment. Looks perfect. Let's get merge!
* Initial commit with barebone SafeVelocityAdjustorNode Signed-off-by: Maxime CLEMENT <[email protected]> * Add debug topics, launch file, and config file Signed-off-by: Maxime CLEMENT <[email protected]> * Fix debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Fix dynamic parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Add proper collision detection and debug footprint Implements Proposal 1. Calculation of the adjusted velocity still needs to be improved Signed-off-by: Maxime CLEMENT <[email protected]> * Add script to compare the original and adjusted velocity profiles Signed-off-by: Maxime CLEMENT <[email protected]> * Fix calculation of distance to obstacle Signed-off-by: Maxime CLEMENT <[email protected]> * Add test for calculation collision distance Signed-off-by: Maxime CLEMENT <[email protected]> * Add launch file to test the safe_velocity_adjustor with a bag Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup code and add tests for forwardSimulatedVector Signed-off-by: Maxime CLEMENT <[email protected]> * Simplify collision detection by not using a footprint polygon Signed-off-by: Maxime CLEMENT <[email protected]> * Add filtering of the dynamic objects from the pointcloud Signed-off-by: Maxime CLEMENT <[email protected]> * [DEBUG] Print runtimes of expensive functions Signed-off-by: Maxime CLEMENT <[email protected]> * Add trajectory downsampling to boost performance + improve debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Modify velocity only from ego pose + distance parameter Signed-off-by: Maxime CLEMENT <[email protected]> * Add 1st Eigen version of distanceToClosestCollision + benchmark Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to using contours from occupancy grid for collision checking Filtering of dynamic objects is not great Signed-off-by: Maxime CLEMENT <[email protected]> * Add buffer around dynamic obstacles to avoid false obstacle detection Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to limit the adjusted velocity Signed-off-by: Maxime CLEMENT <[email protected]> * Use vehicle_info_util to get vehicle footprint Signed-off-by: Maxime CLEMENT <[email protected]> * Calculate accurate distance to collision + add tests Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter for the min velocity where a dynamic obstacle is ignored Signed-off-by: Maxime CLEMENT <[email protected]> * Add README and some pictures to explain the node inner workings Signed-off-by: Maxime CLEMENT <[email protected]> * Update scenario_planning.launch.xml to run the new node Signed-off-by: Maxime CLEMENT <[email protected]> * Fix format of launch files Signed-off-by: Maxime CLEMENT <[email protected]> * Update launcher and rviz config used for debuging with bag Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup debug publishing Signed-off-by: Maxime CLEMENT <[email protected]> * Complete tests of collision_distance.hpp Signed-off-by: Maxime CLEMENT <[email protected]> * Add docstring + Small code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Improve test of occupancy_grid_utils Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug when setting parameter callback before getting vehicle parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Rename safe_velocity_adjustor to apparent_safe_velocity_limiter Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (apparent_safe_velocity_limiter_node) Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (occupancy_grid_utils) Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (collision_distance) Signed-off-by: Maxime CLEMENT <[email protected]> * Add exec of trajectory_visualizer.py in launch files Signed-off-by: Maxime CLEMENT <[email protected]> * Mask trajectory footprint from the occupancy grid (might be expensive) Signed-off-by: Maxime CLEMENT <[email protected]> * Filter out the occupancy grid that is outside the envelope polygon Signed-off-by: Maxime CLEMENT <[email protected]> * Add improved PolygonIterator using scan line algorithm Signed-off-by: Maxime CLEMENT <[email protected]> * Use autoware_cmake for dependencies Signed-off-by: Maxime CLEMENT <[email protected]> * Improve performances of PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Minor cleanup of PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Use improved iterator + add benchmark (max/avg/med) to node Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Switch from set to vector/list in PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Remove PolygonIterator and use implementation from grid_map_utils Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to limit deceleration when adjusting the velocity Signed-off-by: Maxime CLEMENT <[email protected]> * Code cleanup, move type decl and debug functions to separate files Signed-off-by: Maxime CLEMENT <[email protected]> * Add support for collision detection using pointcloud Signed-off-by: Maxime CLEMENT <[email protected]> * Code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Speedup pointcloud filtering (still ~100ms on bags) Signed-off-by: Maxime CLEMENT <[email protected]> * Improve envelope calculation and use separate node for pcd downsampling Signed-off-by: Maxime CLEMENT <[email protected]> * Add ProjectionParameters to prepare for the bicycle model projection Signed-off-by: Maxime CLEMENT <[email protected]> * Add bicycle projection with various steering offsets Signed-off-by: Maxime CLEMENT <[email protected]> * Update docstring Signed-off-by: Maxime CLEMENT <[email protected]> * Major refactoring, calculate envelope from footprints Signed-off-by: Maxime CLEMENT <[email protected]> * Add extraction of static obstacles from lanelet map Signed-off-by: Maxime CLEMENT <[email protected]> * Remove stopwatch Signed-off-by: Maxime CLEMENT <[email protected]> * Add arc distance calculation when using bicycle projection Signed-off-by: Maxime CLEMENT <[email protected]> * Fix multi geometry definitions in tier4_autoware_utils/boost_geometry Signed-off-by: Maxime CLEMENT <[email protected]> * Improve geometry operations to take advantage of Eigen Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to min/max offset and simplify footprint calculations Signed-off-by: Maxime CLEMENT <[email protected]> * Fix unit tests (unset params.heading) Signed-off-by: Maxime CLEMENT <[email protected]> * Add option to filter obstacles using the safety envelope Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug with distance calculation and improve debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Update README Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to set map obstacles by linestring id (for debug) Signed-off-by: Maxime CLEMENT <[email protected]> * Move param structures to dedicated file and add PreprocessingParameters Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to calculate steering angle of trajectory points Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup footprint generation Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug with debug marker ids Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug where the VelocityParameters were not constructed Signed-off-by: Maxime CLEMENT <[email protected]> * Update obstacles extraction Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to collision detection using rtree Signed-off-by: Maxime CLEMENT <[email protected]> * Add publishing of the runtime (in microseconds) Signed-off-by: Maxime CLEMENT <[email protected]> * Add option to ignore obstacles on the trajectory Signed-off-by: Maxime CLEMENT <[email protected]> * Add max length and max duration parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Restructure Obstacles structure to separate lines and points for speedup Signed-off-by: Maxime CLEMENT <[email protected]> * Convert obstacle linestrings to segments when used in the rtree Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter for extra distance when filtering the ego path Signed-off-by: Maxime CLEMENT <[email protected]> * Fix issues caused by rebase Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Update to run with looping bag replay Signed-off-by: Maxime CLEMENT <[email protected]> * Add debug markers for obstacle masks and only publish when subscribed Signed-off-by: Maxime CLEMENT <[email protected]> * Update README Signed-off-by: Maxime CLEMENT <[email protected]> * Fix humble build issue with PCL library Signed-off-by: Maxime CLEMENT <[email protected]> * Update obstacle extraction from lanelet map (no longer based on route) Signed-off-by: Maxime CLEMENT <[email protected]> * Optimize use of rtree + use naive collision checking with few obstacles Signed-off-by: Maxime CLEMENT <[email protected]> * Remove debug code and update default parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Do not wait for self pose Signed-off-by: Maxime CLEMENT <[email protected]> * Rename to obstacle_velocity_limiter Signed-off-by: Maxime CLEMENT <[email protected]> * More minor cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Update READEME.md Signed-off-by: Maxime CLEMENT <[email protected]> * Update README to have the purpose written before the illustration Signed-off-by: Maxime CLEMENT <[email protected]> * Update copyright notice: Tier IV -> TIER IV Signed-off-by: Maxime CLEMENT <[email protected]> * Remove use_sim_time param from node launch file Signed-off-by: Maxime CLEMENT <[email protected]> * Update launch files to run in the motion_planner + add launch config Signed-off-by: Maxime CLEMENT <[email protected]> Signed-off-by: Maxime CLEMENT <[email protected]> Signed-off-by: Kotaro Yoshimoto <[email protected]>
* Initial commit with barebone SafeVelocityAdjustorNode Signed-off-by: Maxime CLEMENT <[email protected]> * Add debug topics, launch file, and config file Signed-off-by: Maxime CLEMENT <[email protected]> * Fix debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Fix dynamic parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Add proper collision detection and debug footprint Implements Proposal 1. Calculation of the adjusted velocity still needs to be improved Signed-off-by: Maxime CLEMENT <[email protected]> * Add script to compare the original and adjusted velocity profiles Signed-off-by: Maxime CLEMENT <[email protected]> * Fix calculation of distance to obstacle Signed-off-by: Maxime CLEMENT <[email protected]> * Add test for calculation collision distance Signed-off-by: Maxime CLEMENT <[email protected]> * Add launch file to test the safe_velocity_adjustor with a bag Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup code and add tests for forwardSimulatedVector Signed-off-by: Maxime CLEMENT <[email protected]> * Simplify collision detection by not using a footprint polygon Signed-off-by: Maxime CLEMENT <[email protected]> * Add filtering of the dynamic objects from the pointcloud Signed-off-by: Maxime CLEMENT <[email protected]> * [DEBUG] Print runtimes of expensive functions Signed-off-by: Maxime CLEMENT <[email protected]> * Add trajectory downsampling to boost performance + improve debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Modify velocity only from ego pose + distance parameter Signed-off-by: Maxime CLEMENT <[email protected]> * Add 1st Eigen version of distanceToClosestCollision + benchmark Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to using contours from occupancy grid for collision checking Filtering of dynamic objects is not great Signed-off-by: Maxime CLEMENT <[email protected]> * Add buffer around dynamic obstacles to avoid false obstacle detection Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to limit the adjusted velocity Signed-off-by: Maxime CLEMENT <[email protected]> * Use vehicle_info_util to get vehicle footprint Signed-off-by: Maxime CLEMENT <[email protected]> * Calculate accurate distance to collision + add tests Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter for the min velocity where a dynamic obstacle is ignored Signed-off-by: Maxime CLEMENT <[email protected]> * Add README and some pictures to explain the node inner workings Signed-off-by: Maxime CLEMENT <[email protected]> * Update scenario_planning.launch.xml to run the new node Signed-off-by: Maxime CLEMENT <[email protected]> * Fix format of launch files Signed-off-by: Maxime CLEMENT <[email protected]> * Update launcher and rviz config used for debuging with bag Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup debug publishing Signed-off-by: Maxime CLEMENT <[email protected]> * Complete tests of collision_distance.hpp Signed-off-by: Maxime CLEMENT <[email protected]> * Add docstring + Small code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Improve test of occupancy_grid_utils Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug when setting parameter callback before getting vehicle parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Rename safe_velocity_adjustor to apparent_safe_velocity_limiter Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (apparent_safe_velocity_limiter_node) Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (occupancy_grid_utils) Signed-off-by: Maxime CLEMENT <[email protected]> * Move declarations to cpp file (collision_distance) Signed-off-by: Maxime CLEMENT <[email protected]> * Add exec of trajectory_visualizer.py in launch files Signed-off-by: Maxime CLEMENT <[email protected]> * Mask trajectory footprint from the occupancy grid (might be expensive) Signed-off-by: Maxime CLEMENT <[email protected]> * Filter out the occupancy grid that is outside the envelope polygon Signed-off-by: Maxime CLEMENT <[email protected]> * Add improved PolygonIterator using scan line algorithm Signed-off-by: Maxime CLEMENT <[email protected]> * Use autoware_cmake for dependencies Signed-off-by: Maxime CLEMENT <[email protected]> * Improve performances of PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Minor cleanup of PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Use improved iterator + add benchmark (max/avg/med) to node Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Switch from set to vector/list in PolygonIterator Signed-off-by: Maxime CLEMENT <[email protected]> * Remove PolygonIterator and use implementation from grid_map_utils Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to limit deceleration when adjusting the velocity Signed-off-by: Maxime CLEMENT <[email protected]> * Code cleanup, move type decl and debug functions to separate files Signed-off-by: Maxime CLEMENT <[email protected]> * Add support for collision detection using pointcloud Signed-off-by: Maxime CLEMENT <[email protected]> * Code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Speedup pointcloud filtering (still ~100ms on bags) Signed-off-by: Maxime CLEMENT <[email protected]> * Improve envelope calculation and use separate node for pcd downsampling Signed-off-by: Maxime CLEMENT <[email protected]> * Add ProjectionParameters to prepare for the bicycle model projection Signed-off-by: Maxime CLEMENT <[email protected]> * Add bicycle projection with various steering offsets Signed-off-by: Maxime CLEMENT <[email protected]> * Update docstring Signed-off-by: Maxime CLEMENT <[email protected]> * Major refactoring, calculate envelope from footprints Signed-off-by: Maxime CLEMENT <[email protected]> * Add extraction of static obstacles from lanelet map Signed-off-by: Maxime CLEMENT <[email protected]> * Remove stopwatch Signed-off-by: Maxime CLEMENT <[email protected]> * Add arc distance calculation when using bicycle projection Signed-off-by: Maxime CLEMENT <[email protected]> * Fix multi geometry definitions in tier4_autoware_utils/boost_geometry Signed-off-by: Maxime CLEMENT <[email protected]> * Improve geometry operations to take advantage of Eigen Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to min/max offset and simplify footprint calculations Signed-off-by: Maxime CLEMENT <[email protected]> * Fix unit tests (unset params.heading) Signed-off-by: Maxime CLEMENT <[email protected]> * Add option to filter obstacles using the safety envelope Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug with distance calculation and improve debug markers Signed-off-by: Maxime CLEMENT <[email protected]> * Update README Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to set map obstacles by linestring id (for debug) Signed-off-by: Maxime CLEMENT <[email protected]> * Move param structures to dedicated file and add PreprocessingParameters Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter to calculate steering angle of trajectory points Signed-off-by: Maxime CLEMENT <[email protected]> * Cleanup footprint generation Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug with debug marker ids Signed-off-by: Maxime CLEMENT <[email protected]> * Fix bug where the VelocityParameters were not constructed Signed-off-by: Maxime CLEMENT <[email protected]> * Update obstacles extraction Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Switch to collision detection using rtree Signed-off-by: Maxime CLEMENT <[email protected]> * Add publishing of the runtime (in microseconds) Signed-off-by: Maxime CLEMENT <[email protected]> * Add option to ignore obstacles on the trajectory Signed-off-by: Maxime CLEMENT <[email protected]> * Add max length and max duration parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Restructure Obstacles structure to separate lines and points for speedup Signed-off-by: Maxime CLEMENT <[email protected]> * Convert obstacle linestrings to segments when used in the rtree Signed-off-by: Maxime CLEMENT <[email protected]> * Add parameter for extra distance when filtering the ego path Signed-off-by: Maxime CLEMENT <[email protected]> * Fix issues caused by rebase Signed-off-by: Maxime CLEMENT <[email protected]> * Minor code cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Update to run with looping bag replay Signed-off-by: Maxime CLEMENT <[email protected]> * Add debug markers for obstacle masks and only publish when subscribed Signed-off-by: Maxime CLEMENT <[email protected]> * Update README Signed-off-by: Maxime CLEMENT <[email protected]> * Fix humble build issue with PCL library Signed-off-by: Maxime CLEMENT <[email protected]> * Update obstacle extraction from lanelet map (no longer based on route) Signed-off-by: Maxime CLEMENT <[email protected]> * Optimize use of rtree + use naive collision checking with few obstacles Signed-off-by: Maxime CLEMENT <[email protected]> * Remove debug code and update default parameters Signed-off-by: Maxime CLEMENT <[email protected]> * Do not wait for self pose Signed-off-by: Maxime CLEMENT <[email protected]> * Rename to obstacle_velocity_limiter Signed-off-by: Maxime CLEMENT <[email protected]> * More minor cleanup Signed-off-by: Maxime CLEMENT <[email protected]> * Update READEME.md Signed-off-by: Maxime CLEMENT <[email protected]> * Update README to have the purpose written before the illustration Signed-off-by: Maxime CLEMENT <[email protected]> * Update copyright notice: Tier IV -> TIER IV Signed-off-by: Maxime CLEMENT <[email protected]> * Remove use_sim_time param from node launch file Signed-off-by: Maxime CLEMENT <[email protected]> * Update launch files to run in the motion_planner + add launch config Signed-off-by: Maxime CLEMENT <[email protected]> Signed-off-by: Maxime CLEMENT <[email protected]> Signed-off-by: yoshiri <[email protected]>
…tersection fix(intersection): set RTC enable (autowarefoundation#9040)
Description
This PR adds a new package
obstacle_velocity_limiter
that is used to increase the feeling of safety by limiting the velocity when driving towards obstacles (e.g., in curves).Related links
Tests performed
Notes for reviewers
Pre-review checklist for the PR author
The PR author must check the checkboxes below when creating the PR.
In-review checklist for the PR reviewers
The PR reviewers must check the checkboxes below before approval.
Post-review checklist for the PR author
The PR author must check the checkboxes below before merging.
After all checkboxes are checked, anyone who has write access can merge the PR.